#!/usr/bin/env python3
#coding=utf-8

import datetime 
import threading 
import time 
from typing import List, Set 
from timeit import default_timer as timer
from main_module import UiLog, Database
from sacp_client import SacpClient, FrameInfo, Attribute, SacpType
from PyQt5.QtCore import QObject, QTimer
from loguru import logger as log
from tcp_server import *

def to_version(version_int):
    main = (version_int >> 24) & 0xff
    minor = (version_int >> 16) & 0xff
    patch = (version_int & 0xffff)
    return "V%d.%d.%04d" % (main, minor, patch)

def to_time_string(time_stamp):
    dt = datetime.datetime.fromtimestamp(time_stamp)
    return dt.strftime("%Y-%m-%d %H:%M:%S")

def to_float_string(v):
    return ("%.3f" % v)

def to_uptime(uptime):
    day = uptime / (24 * 60 * 60)
    uptime = uptime % (24 * 60 * 60)
    hour = uptime / (60 * 60)
    uptime = uptime % (60 * 60)
    min = uptime / 60
    sec = uptime % 60

    if int(day) > 0:
        return "%d天%d时%d分%d秒" % (day, hour, min, sec)
    elif int(hour) > 0:
        return "%d时%d分%d秒" % (hour, min, sec)
    elif int(min) > 0:
        return "%d分%d秒" % (min, sec)
    else:
        return "%d秒" % (sec)

def get_float(fs):
    v = 0
    try:
        v = float(fs)
    except Exception as e:
        v = 0.0 
    return v 

def get_int(ints):
    v = 0
    try:
        v = int(ints)
    except Exception as e:
        v  = 0
    return v

def to_int_string(v):
    return ("%d" % v)


class SacpId:
    #  系统状态上报，周期1秒
    GROUP_SYSTEM_STATE = 0
    #  驱动轮实时数据 周期10ms
    GROUP_MOTION = 1
    #  IMU实时数据 周期10ms
    GROUP_IMU = 2
    #  里程计实时数据 周期10ms
    GROUP_ODOMETRY = 3
    #  地检检测数据 周期100ms
    GROUP_GROUND_DETECT = 4
    #  控制电流 周期100ms
    GROUP_CONTROL_CURRENT = 5
    #  红外接收编码 周期50ms
    GROUP_IR_RAW_CODE = 6  
    #  驱动轮PID实时数据 周期10ms  
    GROUP_MOTION_PID = 7
    #  风机PID实时数据 周期40ms
    GROUP_FAN_PID = 8
    #  系统事件组
    GROUP_EVENT = 9

    ## 系统状态
    STATE_ON_DOCK = (1 << 0)
    STATE_CHARGING = (1 << 1)
    STATE_LEFT_WHEEL_OFF = (1 << 2)
    STATE_RIGHT_WHEEL_OFF = (1 << 3)

    STATE_DUSTBIN_INPLACE = (1 << 4)
    STATE_MOP_INPLACE = (1 << 5)
    STATE_WATER_TANK_INPLACE = (1 << 6)
    STATE_UPGRADE_REQUEST = (1 << 7)

    #  碰撞状态
    STATE_FRONT_COLLISION = (1 << 8)
    STATE_LEFT_COLLISION = (1 << 9)
    STATE_RIGHT_COLLISION = (1 << 10)
    STATE_LIDAR_COLLISION = (1 << 11)

    #  地检离地状态
    STATE_FRONT_GROUND_DETECT_ALARM = (1 << 16)
    STATE_LEFT_GROUND_DETECT_ALARM = (1 << 17)
    STATE_RIGHT_GROUND_DETECT_ALARM = (1 << 18)
    STATE_LEFT_MIDDLE_GROUND_DETECT_ALARM = (1 << 19)
    STATE_RIGHT_MIDDLE_GROUND_DETECT_ALARM = (1 << 20)

    # 应用协议标识 
    ATTR_APPLICATION_ID = 1
    # 应用协议版本
    ATTR_APPLICATION_VERSION= 2
    # 硬件标识 
    ATTR_HARDWARE_ID= 3
    # 硬件版本
    ATTR_HARDWARE_VERSION= 4
    # 软件版本
    ATTR_SOFTWARE_VERSION= 5
    # 软件发布时间
    ATTR_SOFTWARE_RELEASED_TIME= 6 
    # BOOT版本
    ATTR_BOOT_VERSION= 7
    # 内核版本
    ATTR_OS_VERSION= 8
    # 上电时间，单位为s
    ATTR_UP_TIME= 9
    # RTC时间
    ATTR_RTC_TIME= 10
    # RTC时区=  可读写
    ATTR_RTC_TIMEZONE= 11
    # CPU使用率
    ATTR_CPU_USAGED= 12
    # 总内存大小 KB
    ATTR_MEM_TOTAL= 13
    # 可用内存大小 KB
    ATTR_MEM_FREE= 14
    # 系统控制 see::SYSTEM_CONTROL
    ATTR_SYSTEM_CONTROL = 20

    #  组ID
    ATTR_GROUP_TAG = 100
    #  设备状态 u32
    ATTR_MACHINE_STATE = 101
    #  电池电量 百分比
    ATTR_BATTERY_LEVEL = 102
    #  电池电压 单位为V
    ATTR_BATTERY_VOLTAGE = 103
    # 标称电压 单位为V
    #ATTR_BATTERY_NOMINAL_VOLTAGE = 104
    # 标称容量单位为WH
    #ATTR_BATTERY_NOMINAL_CAPACITY = 105
    #  左轮
    ATTR_LEFT_WHEEL_SET_VELOCITY = 110
    ATTR_LEFT_WHEEL_VELOCITY = 111
    ATTR_LEFT_WHEEL_CURRENT = 112
    # u8
    ATTR_LEFT_WHEEL_STATE = 113
    ATTR_LEFT_WHEEL_SET_PWM = 114
    ATTR_LEFT_WHEEL_PID_TARGET = 117
    ATTR_LEFT_WHEEL_PID_ERROR = 118
    ATTR_LEFT_WHEEL_PID_OUTPUT = 119

    #  右轮
    ATTR_RIGHT_WHEEL_SET_VELOCITY = 120
    ATTR_RIGHT_WHEEL_VELOCITY = 121
    ATTR_RIGHT_WHEEL_CURRENT = 122
    # u8
    ATTR_RIGHT_WHEEL_STATE = 123
    ATTR_RIGHT_WHEEL_SET_PWM = 124
    ATTR_RIGHT_WHEEL_PID_TARGET = 127
    ATTR_RIGHT_WHEEL_PID_ERROR = 128
    ATTR_RIGHT_WHEEL_PID_OUTPUT = 129

    #  轮子配置参数
    ATTR_WHEEL_DIAMETER = 130
    ATTR_WHEEL_ENCODER_ACCURACY = 131
    ATTR_WHEEL_GEAR_RATIO = 132
    ATTR_WHEEL_TRACK_SIZE = 133
    # u16
    ATTR_WHEEL_DEBUG_SETTING = 135
    ATTR_WHEEL_PID_LIMITED = 136
    ATTR_WHEEL_PID_KP = 137
    ATTR_WHEEL_PID_KI = 138
    ATTR_WHEEL_PID_KD = 139

    # 其他电机
    ATTR_MAIN_BRUSH_SET_PWM = 140
    ATTR_MAIN_BRUSH_CURRENT = 141
    ATTR_LEFT_SIDE_BRUSH_SET_PWM = 142
    ATTR_LEFT_SIDE_BRUSH_CURRENT = 143
    ATTR_RIGHT_SIDE_BRUSH_SET_PWM = 144
    ATTR_RIGHT_SIDE_BRUSH_CURRENT = 145
    ATTR_WATER_PUMP_SET_PWM = 146
    ATTR_WATER_PUMP_CURRENT = 147
    
    #  IMU
    ATTR_IMU_GYROSCOPE_X = 150
    ATTR_IMU_GYROSCOPE_Y = 151
    ATTR_IMU_GYROSCOPE_Z = 152
    ATTR_IMU_ACCELEROMETER_X = 153
    ATTR_IMU_ACCELEROMETER_Y = 154
    ATTR_IMU_ACCELEROMETER_Z = 155

    #  ODOMETRY
    ATTR_ODOMETRY_X = 160
    ATTR_ODOMETRY_Y = 161
    ATTR_ODOMETRY_Z = 162
    ATTR_ODOMETRY_YAW = 163
    ATTR_ODOMETRY_PITCH = 164
    ATTR_ODOMETRY_ROLL = 165
    ATTR_ODOMETRY_LEFT_WHEEL_DISTANCE = 166
    ATTR_ODOMETRY_RIGHT_WHELL_DISTANCE = 167
    
    # 风机 
    ATTR_FAN_SET_SPEED = 170
    ATTR_FAN_SPEED = 171
    ATTR_FAN_CURRENT = 172
    ATTR_FAN_SET_PWM = 173
    ATTR_FAN_SPEED_RATIO = 174
    ATTR_FAN_DEBUG_SETTING = 175   
    ATTR_FAN_PID_LIMITED = 176   
    ATTR_FAN_PID_KP = 177
    ATTR_FAN_PID_KI = 178
    ATTR_FAN_PID_KD = 179  

    # 风机调试设定
    ATTR_FAN_PID_TARGET = 190
    ATTR_FAN_PID_ERROR = 191
    ATTR_FAN_PID_OUTPUT = 192  
    
    # 地检 - all u16
    ATTR_GROUND_DETECT_FRONT = 180
    ATTR_GROUND_DETECT_LEFT = 181
    ATTR_GROUND_DETECT_RIGHT = 182
    ATTR_GROUND_DETECT_LEFT_MIDDLE = 183
    ATTR_GROUND_DETECT_RIGHT_MIDDLE = 184
    ATTR_GROUND_DETECT_FRONT_THRESHOLD = 185
    ATTR_GROUND_DETECT_LEFT_THRESHOLD = 186
    ATTR_GROUND_DETECT_RIGHT_THRESHOLD = 187
    ATTR_GROUND_DETECT_LEFT_MIDDLE_THRESHOLD = 188
    ATTR_GROUND_DETECT_RIGHT_MIDDLE_THRESHOLD = 189

    # 红外回充与遥控
    ATTR_IR_CHARGER_DOCK_CODE = 200
    ATTR_IR_REMOTE_CODE = 201
    ATTR_IR_REMOTE_EXTRA_CODE = 202 
    ATTR_IR_FRONT_LEFT_RAW_CODE = 205 
    ATTR_IR_FRONT_RIGHT_RAW_CODE = 206
    ATTR_IR_RIGHT_RAW_CODE = 207   
    ATTR_IR_LEFT_REMOTE_RAW_CODE = 208   
    ATTR_IR_RIGHT_REMOTE_RAW_CODE = 209  

    def __init__(self, id):
        self._id = id
    
    def id(self):
        return self._id 


class SacpCounter:
    def __init__(self):
        self.report_frames = 0
        self.read_frames = 0
        self.read_failed_frames = 0
        self.write_frames = 0
        self.write_failed_frames = 0
    
    def clear(self):
        self.report_frames = 0
        self.read_frames = 0
        self.read_failed_frames = 0
        self.write_frames = 0
        self.write_failed_frames = 0

class MachineParameter:
    def __init__(self):
        self.wheel_diameter = 0.0
        self.wheel_gear_ratio = 0.0        
        self.wheel_encoder_accuracy = 0.0
        self.wheel_track_size = 0.0
        self.fan_speed_ratio = 0.0 
    
    
"""
sacp robot的对象

创建时，传参： Database和 LogThead 

提供以下方法：
 
 启动  开启成功，返回True 否则返回 False 
 关闭  如果在运行中，中断接收线程，关闭事件
 
 探测  发送读属性，等待响应，如果成功，表示设备活动，如果失败，表示设备不活动 
 
 读特定属性集的方法 
 写特定属性集的方法 
 
 上报报文处理：
    解释数据成功，写入到Database中 形成闭环即可  

"""

class SacpRobot:
    def __init__(self, log:UiLog, db:Database, alive_check_period=2.0):
        #super(SacpRobot, self).__init__()
        self.uiLog = log 
        self.db = db 
        self.thread = None
        self.client = None          
        self.running = False
        self.counters = SacpCounter() 
        self.alive = False
        self.alive_thread = None 
        self.alive_check_period = alive_check_period
        # self.aliveTimer = QTimer()
        # self.aliveTimer.timeout.connect(self.alive_check)

    def sync_db(self, attrs:List[Attribute], debug=False):
        """
        将属性存放到数据库中
        """
        for attr in attrs:
            if attr.get_id() == SacpId.ATTR_GROUP_TAG:
                continue 
            else:
                if debug:
                    log.debug("set %s" % str(attr))
                self.db.set(attr.get_id(), attr.get_value())       
    
    def clear_counters(self):
        self.counters.clear()
    
    def start(self, port, rate) -> bool:        
        """启动SACP客户端

        Returns:
            bool: 返回成功或失败
        """
        if self.running:
            return False 
        self.client = SacpClient(port, rate, debug=False)
        self.thread = threading.Thread(target=self.__thread_main)
        if self.client.start():
            #self.clear_counters()
            self.uiLog.log("启动SACP客户端(%s/%d)成功" % (port, rate))
            self.thread.start()   
            ## 开始一个新的线程来检查设备是否活动的
            self.alive_thread = threading.Thread(target=self.__alive_check_thread_main)
            self.alive_thread.start()
            return True 
        return False 

    def stop(self):
        """停止运行中的SACP客户端
        """
        if self.client and self.running:            
            self.client.stop()
            self.running = False 
            self.alive = False
            self.thread.join()
            self.alive_thread.join()

    def is_running(self)->bool:
        """SACP客户端是否在运行中

        Returns:
            bool:  
        """
        return self.running 
    
    def is_alive(self)->bool:
        """返回SACP服务端是否活动的

        Returns:
            bool: _description_
        """
        return self.alive
        
    def __thread_main(self):
        self.running = True 
        while self.running:
            f:FrameInfo = self.client.get_report(0.1)
            if f:
                self.counters.report_frames += 1   
                ## 上报报文中，只有事件组，可以启用调试信息
                self.sync_db(f.get_attributes())
        ## 线程结束
        self.running = False
        self.client.stop()
        self.uiLog.log("SACP客户端已停止")    

    def __alive_check_thread_main(self):        
        expired = timer()
        while self.running:
            now = timer()
            if now >= expired:
                self.__alive_check()
                expired = now + self.alive_check_period
            time.sleep(0.1) 

    def __alive_check(self):                
        newState = self.alive
        appId = 0
        if self.running:
            newState, appId = self.probe()
        else:
            newState = False 
        # 仅在状态切换时，输出日志
        if newState != self.alive:
            self.alive = newState 
            if self.alive:
                self.uiLog.log("设备连接成功, 应用ID:%d" % appId)
            else :
                self.uiLog.log("设备连接断开") 

    def read(self, attrs:List[Attribute])->List[Attribute]:
        """读取指定属性

        Args:
            attrs (List[Attribute]): 输入要读取的属性ID及类型

        Returns:
            List[Attribute]: 返回空列表或属性列表
        """
        if not self.running:
            return [] 
        ret = self.client.read_attributes(attrs)
        self.counters.read_frames += 1
        if ret:
            return ret
        self.counters.read_failed_frames += 1
        return []

    def write(self, attrs:List[Attribute])->List[Attribute]:
        """写指定属性

        Args:
            attrs (List[Attribute]): 写属性的值

        Returns:
            List[Attribute]: 返回的写状态
        """
        if not self.running:
            return []
        ret = self.client.write_attributes(attrs)
        self.counters.write_frames += 1
        if ret:
            return ret
        self.counters.write_failed_frames += 1
        return []

    def probe(self):
        """发送一个探测报文，如果收到响应，表示设备在线 

        Returns:
            bool: 
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_APPLICATION_ID, SacpType.TYPE_UINT16))
        attrs.append(Attribute(SacpId.ATTR_APPLICATION_VERSION, SacpType.TYPE_UINT16))
        ret = self.read(attrs)
        if len(ret) == 0:
            return False, 0
        ## 返回应用ID
        appId = ret[0].get_value()
        return True, appId 
        
    def update_version_info(self)->bool:
        """更新设备的版本信息

        Returns:
            bool: 返回更新结果
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_HARDWARE_ID, SacpType.TYPE_UINT32))
        attrs.append(Attribute(SacpId.ATTR_HARDWARE_VERSION, SacpType.TYPE_UINT32))
        attrs.append(Attribute(SacpId.ATTR_SOFTWARE_VERSION, SacpType.TYPE_UINT32))
        attrs.append(Attribute(SacpId.ATTR_SOFTWARE_RELEASED_TIME, SacpType.TYPE_UINT32))      
        ret = self.read(attrs)
        if len(ret) == 0:
            return False 
        self.sync_db(ret, debug=True)
        return True 

    def get_machine_parameters(self)->bool:
        """请求读取轮子参数

        Returns:
            bool: 返回更新结果
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_WHEEL_DIAMETER, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_GEAR_RATIO, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_ENCODER_ACCURACY, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_TRACK_SIZE, SacpType.TYPE_FLOAT))  
        attrs.append(Attribute(SacpId.ATTR_FAN_SPEED_RATIO, SacpType.TYPE_FLOAT))              
        ret = self.read(attrs)
        if len(ret) == 0:
            return False 
        self.sync_db(ret, debug=True)
        return True 

    def set_machine_parameters(self, parameter:MachineParameter)->bool:
        """配置轮子参数

        Args:
            diameter (_type_): _description_
            gear_ratio (_type_): _description_
            encoder_accuracy (_type_): _description_
            track_size (_type_): _description_

        Returns:
            bool: _description_
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_WHEEL_DIAMETER, SacpType.TYPE_FLOAT, parameter.wheel_diameter))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_GEAR_RATIO, SacpType.TYPE_FLOAT, parameter.wheel_gear_ratio))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_ENCODER_ACCURACY, SacpType.TYPE_FLOAT, parameter.wheel_encoder_accuracy))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_TRACK_SIZE, SacpType.TYPE_FLOAT, parameter.wheel_track_size))       
        attrs.append(Attribute(SacpId.ATTR_FAN_SPEED_RATIO, SacpType.TYPE_FLOAT, parameter.fan_speed_ratio))                
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 

    def get_ground_detect_threshold(self)->bool:
        """请求读取地检的阈值

        Returns:
            bool: 返回更新结果
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_FRONT_THRESHOLD, SacpType.TYPE_UINT16))
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_LEFT_THRESHOLD, SacpType.TYPE_UINT16))
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_RIGHT_THRESHOLD, SacpType.TYPE_UINT16))        
        ret = self.read(attrs)
        if len(ret) == 0:
            return False 
        self.sync_db(ret, debug=True)
        return True 

    def set_ground_detect_threshold(self, front, left, right)->bool:
        """设置地检阈值
        Args:
            front (_type_): _description_
            left (_type_): _description_
            right (_type_): _description_

        Returns:
            bool: _description_
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_FRONT_THRESHOLD, SacpType.TYPE_UINT16, int(front)))
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_LEFT_THRESHOLD, SacpType.TYPE_UINT16, int(left)))
        attrs.append(Attribute(SacpId.ATTR_GROUND_DETECT_RIGHT_THRESHOLD, SacpType.TYPE_UINT16, int(right)))
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 

    def get_wheel_pid(self)->bool:
        """请求读取轮子PID设置

        Returns:
            bool: 返回更新结果
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KP, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KI, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KD, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_LIMITED, SacpType.TYPE_FLOAT))      
        ret = self.read(attrs)
        if len(ret) == 0:
            return False 
        self.sync_db(ret, debug=True)
        return True 
    
    def set_wheel_pid(self, kp, ki, kd, limited)->bool:
        """_summary_

        Args:
            kp (_type_): _description_
            ki (_type_): _description_
            kd (_type_): _description_
            limited (_type_): _description_

        Returns:
            bool: _description_
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KP, SacpType.TYPE_FLOAT, kp))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KI, SacpType.TYPE_FLOAT, ki))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_KD, SacpType.TYPE_FLOAT, kd))
        attrs.append(Attribute(SacpId.ATTR_WHEEL_PID_LIMITED, SacpType.TYPE_FLOAT, limited))        
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 
    
    def get_fan_pid(self)->bool:
        """请求读取风机PID设置

        Returns:
            bool: 返回更新结果
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KP, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KI, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KD, SacpType.TYPE_FLOAT))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_LIMITED, SacpType.TYPE_FLOAT))      
        ret = self.read(attrs)
        if len(ret) == 0:
            return False 
        self.sync_db(ret, debug=True)
        return True 
    
    def set_fan_pid(self, kp, ki, kd, limited)->bool:
        """设置风机的PID参数

        Args:
            kp (_type_): _description_
            ki (_type_): _description_
            kd (_type_): _description_
            limited (_type_): _description_

        Returns:
            bool: _description_
        """
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KP, SacpType.TYPE_FLOAT, kp))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KI, SacpType.TYPE_FLOAT, ki))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_KD, SacpType.TYPE_FLOAT, kd))
        attrs.append(Attribute(SacpId.ATTR_FAN_PID_LIMITED, SacpType.TYPE_FLOAT, limited))        
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 
    
    def with_value(self, v):
        if not v and v != 0:
            return False
        else:
            return True
    
    def drive_wheel_velocity(self, left=None, right=None)->bool:
        attrs = []
        if self.with_value(left):
            attrs.append(Attribute(SacpId.ATTR_LEFT_WHEEL_SET_VELOCITY, SacpType.TYPE_FLOAT, left))

        if self.with_value(right):
            attrs.append(Attribute(SacpId.ATTR_RIGHT_WHEEL_SET_VELOCITY, SacpType.TYPE_FLOAT, right))     
        
        if len(attrs) == 0:
            return False 
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 

    def drive_wheel_pwm(self, left=None, right=None)->bool:
        attrs = []
        if self.with_value(left):
            attrs.append(Attribute(SacpId.ATTR_LEFT_WHEEL_SET_PWM, SacpType.TYPE_FLOAT, left))
            
        if self.with_value(right):
            attrs.append(Attribute(SacpId.ATTR_RIGHT_WHEEL_SET_PWM, SacpType.TYPE_FLOAT, right))     
        
        if len(attrs) == 0:
            return False 
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 

    def drive_fan_speed(self, speed)->bool:
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_FAN_SET_SPEED, SacpType.TYPE_FLOAT, speed))
        
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 

    def drive_fan_pwm(self, pwm)->bool:
        attrs = []
        attrs.append(Attribute(SacpId.ATTR_FAN_SET_PWM, SacpType.TYPE_FLOAT, pwm))
            
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 
        
    def drive_motor_pwm(self, attr, pwm)->bool:
        attrs = []
        attrs.append(Attribute(attr, SacpType.TYPE_FLOAT, pwm))
            
        ret = self.write(attrs)
        if len(ret) == 0:
            return False 
        return True 
        
            